#include "./pick_door.h"
#include "../device/gpio.h"
#include "./sensor.h"
#include "../utils/progress.h"
#include "../analog.h"

namespace driver {

using namespace device::gpio;
using namespace utils;

using DIR1 = PC<8>;
using DIR2 = PC<9>;
using PICK1 = PC<10>;
using PICK2 = PC<11>;
using STRAW = PC<12>;

PickDoor::PickDoor() {

    RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;

    config_pp_out<DIR1, DIR2, PICK1, PICK2, STRAW>();
    set_dir(Dir::BRAKE);
}

Error PickDoor::open1() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_pick1_open()) {
        goto finish;
    }

    set_dir(Dir::OPEN);
    PICK1::set();

    for (;;) {

        if (sensor->is_pick1_open()) {
            break;
        }

        if (prog.is_after(TIMEOUT_DURATION)) {
            err = ec::PICK_DOOR_MOTOR1_TIMEOUT;
            break;
        }

        if (prog.is_after(LOCK_DELAY) && analog->is_pick_motor_locked()) {
            lock_count ++;
            if (lock_count >= LOCK_MAX_COUNT) {
                err = ec::PICK_DOOR_MOTOR1_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }

        osDelay(20);
    }

finish:
    set_dir(Dir::BRAKE);
    return err;
}

Error PickDoor::close1() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_pick1_close()) {
        goto finish;
    }

    set_dir(Dir::CLOSE);
    PICK1::set();

    for (;;) {

        if (sensor->is_pick1_close()) {
            break;
        }

        if (prog.is_after(TIMEOUT_DURATION)) {
            err = ec::PICK_DOOR_MOTOR1_TIMEOUT;
            break;
        }

        if (prog.is_after(LOCK_DELAY) && analog->is_pick_motor_locked()) {
            lock_count ++;
            if (lock_count >= LOCK_MAX_COUNT) {
                err = ec::PICK_DOOR_MOTOR1_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }

        if (sensor->is_pick1_guard()) {
            set_dir(Dir::BRAKE);
            osDelay(500);
            open1();
            err = ec::PICK_DOOR_MOTOR1_GUARD;
            break;
        }

        osDelay(20);
    }

finish:
    set_dir(Dir::BRAKE);
    return err;
}

Error PickDoor::open2() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_pick2_open()) {
        goto finish;
    }
    set_dir(Dir::OPEN);
    PICK2::set();

    for (;;) {

        if (sensor->is_pick2_open()) {
            break;
        }

        if (prog.is_after(TIMEOUT_DURATION)) {
            err = ec::PICK_DOOR_MOTOR2_TIMEOUT;
            break;
        }

        if (prog.is_after(LOCK_DELAY) && analog->is_pick_motor_locked()) {
            lock_count ++;
            if (lock_count >= LOCK_MAX_COUNT) {
                err = ec::PICK_DOOR_MOTOR2_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }

        osDelay(20);
    }

finish:
    set_dir(Dir::BRAKE);
    return err;
}

Error PickDoor::close2() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_pick2_close()) {
        goto finish;
    }
    set_dir(Dir::CLOSE);
    PICK2::set();

    for (;;) {

        if (sensor->is_pick2_close()) {
            break;
        }

        if (prog.is_after(TIMEOUT_DURATION)) {
            err = ec::PICK_DOOR_MOTOR2_TIMEOUT;
            break;
        }

        if (prog.is_after(LOCK_DELAY) && analog->is_pick_motor_locked()) {
            lock_count ++;
            if (lock_count >= LOCK_MAX_COUNT) {
                err = ec::PICK_DOOR_MOTOR2_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }

        if (sensor->is_pick2_guard()) {
            set_dir(Dir::BRAKE);
            osDelay(500);
            open2();
            err = ec::PICK_DOOR_MOTOR2_GUARD;
            break;
        }

        osDelay(20);
    }

finish:
    set_dir(Dir::BRAKE);
    return err;
}

Error PickDoor::open_straw() {
    Error err;
    if (!sensor->is_elock_close()) {
        err = ec::PICK_STRAW_OPEN_FAIL;
        goto finish;
    }
    set_dir(Dir::STRAW_OPEN);
    STRAW::set();
    osDelay(500);
    set_dir(Dir::BRAKE);
finish:
    return err;
}

Error PickDoor::close_straw() {
    Error err;
    if (!sensor->is_elock_close()) {
        err = ec::PICK_STRAW_CLOSE_FAIL;
        goto finish;
    }
    set_dir(Dir::STRAW_CLOSE);
    STRAW::set();
    osDelay(500);
    set_dir(Dir::BRAKE);
finish:
    return err;
}

void PickDoor::set_dir(Dir dir) {
    switch (dir) {
        case Dir::OPEN:
            DIR1::set();
            DIR2::clr();
        break;

        case Dir::CLOSE:
            DIR1::clr();
            DIR2::set();
        break;

        case Dir::BRAKE:
            DIR1::clr();
            DIR2::clr();
            PICK1::clr();
            PICK2::clr();
            STRAW::clr();
        break;
    }
}


}

